/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#!/usr/bin/python
# Example of Position server in python

import sys, threading, math, time
sys.path.append("../../../../GIMI/wrap/Python/lib/")
sys.path.append("../../../interfaces/wrap/Python/lib/")
import PositionServer as PS
import gimi


def parseArgs(args):
	parsedArgs = {}
	nextIsValueOf = ""
	for arg in args:
		if not arg:
			continue
		if arg[0] == '-':
			nextIsValueOf = arg[1:]
			parsedArgs[nextIsValueOf] = ""
		else:
			if nextIsValueOf != "":
				parsedArgs[nextIsValueOf] = arg
				nextIsValueOf = ""
	return parsedArgs



def printUsage():
	print """
This is an example of how to use wrapped Position Server with Python.
 
Usage is:	%s [OPTIONS]
  [-u addr]			Use 'addr' as GIMnet AccessPoints address
  [-p port]			Use 'port' as GIMnet AccessPoints port
  [-n name]			Use 'name' as this nodes GIMnet name
  [-G name]			Specify the MaCIGroup for this module
  [-I name]			Specify the MaCIGroup Interface Instance

  
  [-v]				Increase verbosity by 1
  [-q]				Quiet mode
  [-h]				print this
	"""%sys.argv[0]

		
name = "testPyPoseServer"
groupName = "hinkuvinku"
instanceName = "suurvisiiri"
server = "asrobo.hut.fi"
port = 40002

args = parseArgs(sys.argv)
print args

if args.has_key("h"):
	printUsage()
	sys.exit()

if args.has_key("u"):server = args['u']
if args.has_key("p"):port = int(args['p'])
if args.has_key("n"):name = args['n']
if args.has_key("G"):groupName = args['G']
if args.has_key("I"):instanceName = args['I']



#MaCI connection
g = gimi.GIMI()
mcs = PS.CMaCICtrlServer(g)
ps = PS.CPositionServer(mcs, 0)

mcs.SetGroupName(groupName) 
ps.SetInterfaceInstanceName(instanceName)


print "Connecting to %s:%s as %s..."%(server, port, name), 
ret = g.connectToHubEx(server, port, name)
if ret != PS.KMaCIOK:
	print "Failed: ", PS.GetErrorStr(ret)
	sys.exit()
else: print PS.GetErrorStr(ret)



print "Opening... ",
ret = ps.Open()
if ret != PS.KMaCIOK:
	print "Failed: ", PS.GetErrorStr(ret)
	sys.exit()
else: print PS.GetErrorStr(ret)
del ret
#time.sleep(1.0) # got to sleep to wait for something to get ready to serve us


print "start serving"
r = 1.0
t = math.pi
i = 0.01
d = 0.1

poseData = PS.CPositionData()
poseData.CreateInternalBinBag()
var = PS.TPose2D(r, i, d)
v = math.tan(i)/d
ts = PS.TTimestamp()
speed = PS.TSpeed2D(v, i/d)
try:
	while True:
		time.sleep(d)
		ts.SetToCurrentTime()
		t += i
		x = r*math.cos(t)
		y = r*math.sin(t)
		theta = t + math.pi/2.
		#print x, y, theta
		pose = PS.TPose2D(x, y, theta)
		poseData.Reset()
		poseData.SetTimestamp(ts)
		poseData.SetSpeed2D(speed)
		poseData.SetVariance2D(var)
		poseData.SetPose2D(pose)
		ps.SendPositionEvent(poseData)
except KeyboardInterrupt:
	print "\nClosing %s"%name
ps.Close()



